(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[     94452,       2760]
NotebookOptionsPosition[     94178,       2746]
NotebookOutlinePosition[     94537,       2762]
CellTagsIndexPosition[     94494,       2759]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"2", "d", " ", "kneed", " ", "biped", " ", "with", " ", "torso"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"with", " ", "all", " ", "output", " ", "functions"}], "*)"}], 
  "\[IndentingNewLine]", "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "Linearize.m"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
       RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}],
       "]"}], ";"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"choose", " ", "the", " ", "output", " ", "combination"}], "*)"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", " ", "1"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"IndexAllPre", " ", "=", " ", 
     RowBox[{"Import", "[", 
      RowBox[{"\"\<data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"IndexAll", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"HipPosIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"NSslopeIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoIndex", "=", 
     RowBox[{"Round", "[", 
      RowBox[{"IndexAll", "[", 
       RowBox[{"[", 
        RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}], ";"}], "\n", 
   
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"constsubs", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Lc", "\[Rule]", 
         FractionBox["43561", "100000"]}], ",", 
        RowBox[{"Lt", "\[Rule]", 
         FractionBox["40134", "100000"]}], ",", 
        RowBox[{"LT", "\[Rule]", 
         FractionBox["3945", "10000"]}], ",", 
        RowBox[{"mh", "\[Rule]", 
         FractionBox["615", "10"]}], ",", 
        RowBox[{"mt", "\[Rule]", 
         FractionBox["91", "10"]}], ",", 
        RowBox[{"mc", "\[Rule]", 
         FractionBox["4218", "1000"]}], ",", 
        RowBox[{"mf", "\[Rule]", 
         FractionBox["1315", "1000"]}], ",", 
        RowBox[{"g", "\[Rule]", 
         FractionBox["981", "100"]}]}], "}"}]}], ";"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"constsubs", "=", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Lc", "\[Rule]", 
         FractionBox["43561", "100000"]}], ",", 
        RowBox[{"Lt", "\[Rule]", 
         FractionBox["4195", "100000"]}], ",", 
        RowBox[{"LT", "\[Rule]", 
         FractionBox["3751", "10000"]}], ",", 
        RowBox[{"mh", "\[Rule]", 
         FractionBox["615", "10"]}], ",", 
        RowBox[{"mt", "\[Rule]", 
         FractionBox["91", "10"]}], ",", 
        RowBox[{"mc", "\[Rule]", 
         FractionBox["4218", "1000"]}], ",", 
        RowBox[{"mf", "\[Rule]", 
         FractionBox["1315", "1000"]}], ",", 
        RowBox[{"g", "\[Rule]", 
         FractionBox["981", "100"]}]}], "}"}]}], ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["3806", "10000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["4522", "10000"]}], ",", 
       RowBox[{"LT", "\[Rule]", 
        FractionBox["3273", "10000"]}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["471", "10"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["694", "100"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["323", "100"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["1006", "1000"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"mean", " ", "human", " ", "model"}], "*)"}], "\n", 
    RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"mm", "=", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
        "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"statesubs", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", "i", "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "i"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", 
           RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"mm", "=", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "mf", ",", "mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc", ",", 
         "mf"}], "}"}], "/.", "constsubs"}]}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"p0", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", " ", "0"}], ",", 
       
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}], ",", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", " ", 
        "0"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], 
   RowBox[{"(*", 
    RowBox[{"positive", " ", "z", " ", "direction"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q5"], "=", 
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"1", "-", 
             RowBox[{"433", "/", "1000"}]}], ")"}], "Lt"}]}]}], "}"}]}], 
      "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "6"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{"1", "-", 
            RowBox[{"433", "/", "1000"}]}], ")"}], "Lc"}]}], "}"}]}], "]"}]}],
     ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "7"]], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}],
     "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "6"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "7"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{"(*", "aniplot", "*)"}], "\n", 
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"1", ",", "3"}], "}"}], ",", 
           RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
        "]"}], "/.", "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            RowBox[{
             SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
            "\[LeftDoubleBracket]", 
            RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"2", "*", 
              RowBox[{"(", 
               RowBox[{"mf", "+", "mc", "+", "mt"}], ")"}]}], "+", "mh"}], 
            ")"}]}]}]}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "kinetic", " ", "energy", " ", "and", " ", 
     "manipulator", " ", "inertia", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["v", "i"], "=", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{"t", ",", "1"}], "}"}]], 
          RowBox[{"RigidPosition", "[", 
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], "]"}]}], "/.", 
         "\[InvisibleSpace]", "constsubs"}], "]"}]}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"T", "=", 
     RowBox[{
      FractionBox["1", "2"], " ", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         SubscriptBox["v", "i"], ".", 
         SubscriptBox["v", "i"]}]}]}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "dqe", "]"}], ",", "2"}], "}"}]], "T"}], "/.",
        "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
     "*)"}], "\n", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
         "\[RightDoubleBracket]"}]}], "]"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "\[Rule]", ".923"}]}], "}"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
        "\[InvisibleSpace]", "p0"}], "/.", "constsubs"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
      RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], "\n", 
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "\n", 
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
      RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
      RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"LinearNStorsoPre", "=", 
       RowBox[{"Normal", "[", 
        RowBox[{"Series", "[", 
         RowBox[{
          RowBox[{
           FractionBox[
            RowBox[{
             RowBox[{
              RowBox[{
               SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
             RowBox[{
              RowBox[{
               SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
            RowBox[{
             RowBox[{
              RowBox[{
               SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
             RowBox[{
              RowBox[{
               SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
           "p0"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}]}], "]"}], "]"}]}], ";", 
      RowBox[{"(*", 
       RowBox[{
        RowBox[{"Linearized", " ", "torso", " ", "non"}], "-", 
        RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
      RowBox[{"pt1", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt2", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt3", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt4", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt5", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\n", 
      RowBox[{"LinearNStorso", " ", "=", " ", 
       RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}]}], 
     "*)"}], "\n", 
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
     "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
     "*)"}], "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"LinearStorsoPre", "=", 
       RowBox[{"Normal", "[", 
        RowBox[{"Series", "[", 
         RowBox[{
          RowBox[{
           FractionBox[
            RowBox[{
             RowBox[{
              SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
            RowBox[{
             RowBox[{
              SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]], "/.", 
           "p0"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", 
            "1"}], "}"}]}], "]"}], "]"}]}], ";", 
      RowBox[{"(*", 
       RowBox[{"Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
       "*)"}], "\n", 
      RowBox[{"pt1", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt2", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt3", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearStorsoPre", 
          StyleBox[",",
           FontWeight->"Plain"], 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt4", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt5", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearStorsoPre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\n", 
      RowBox[{"LinearStorso", "=", " ", 
       RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}]}], 
     "*)"}], "\[IndentingNewLine]", 
    RowBox[{"(*", 
     RowBox[{"non", "-", 
      RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"1", " ", "non"}], "-", 
      RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"linearized", " ", "non"}], "-", 
      RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"LinearNSslopePre", "=", 
       RowBox[{"Simplify", "[", 
        RowBox[{"Normal", "[", 
         RowBox[{"Series", "[", 
          RowBox[{
           RowBox[{
            FractionBox[
             RowBox[{
              RowBox[{
               RowBox[{
                SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
               "\[LeftDoubleBracket]", 
               RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
              RowBox[{
               RowBox[{
                SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
               "\[LeftDoubleBracket]", 
               RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
             RowBox[{
              RowBox[{
               RowBox[{
                SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
               "\[LeftDoubleBracket]", 
               RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
              RowBox[{
               RowBox[{
                SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
               "\[LeftDoubleBracket]", 
               RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
            "p0"}], ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", "0", ",", 
             "1"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", "0", ",", 
             "1"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", "0", ",", 
             "1"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", "0", ",", 
             "1"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", "0", ",", 
             "1"}], "}"}]}], "]"}], "]"}], "]"}]}], ";", 
      RowBox[{"(*", 
       RowBox[{
        RowBox[{"linearized", " ", "non"}], "-", 
        RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
      RowBox[{"pt1", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNSslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt2", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNSslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt3", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNSslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt4", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNSslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\[IndentingNewLine]", 
      RowBox[{"pt5", " ", "=", " ", 
       RowBox[{
        RowBox[{"Collect", "[", 
         RowBox[{"LinearNSslopePre", ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], "/.", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}], ",", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", " ", 
           "0"}]}], "}"}]}]}], ";", "\n", 
      RowBox[{"LinearNSslope", "=", " ", 
       RowBox[{"pt1", "+", "pt2", "+", "pt3", "+", "pt4", "+", "pt5"}]}], 
      ";"}], "*)"}], "\n", 
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"hip", " ", "angle"}], "*)"}], "\n", 
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"Hip", " ", "Position"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"LHipPos", " ", "=", " ", 
      RowBox[{
       RowBox[{
        RowBox[{"-", "Lc"}], " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
       RowBox[{"Lt", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}]}]}], 
     ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"Linearized", " ", "Hip", " ", "Position"}], "*)"}], "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{
       RowBox[{"-", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ")"}]}], ";"}], 
     "*)"}], 
    RowBox[{"(*", 
     RowBox[{
     "Dimensionless", " ", "Linearized", " ", "Hip", " ", "Position"}], 
     "*)"}], "\n", 
    RowBox[{"(*", 
     RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Hip", " ", "Position", " ", 
      RowBox[{"Output", ":", 
       RowBox[{"1.", " ", "hip", " ", "position"}]}]}], ";", 
     RowBox[{
     "2.", " ", "Linearized", " ", "hip", " ", "position", " ", "3.", " ", 
      "Dimensionless", " ", "Linearized", " ", "hip", " ", "position"}]}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"time", "-", 
      RowBox[{"invariant", " ", 
       RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"calculate", 
      FormBox[
       SubscriptBox["y", "d"],
       TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], 
    "\n", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";"}], "\[IndentingNewLine]", " ", "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso"}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"zero", " ", "dynamics"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Zstatesubs", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"z", ",", " ", "1"}], "]"}], "[", "t", "]"}], " ", "->", 
         " ", 
         FormBox[
          RowBox[{"z", "[", "1", "]"}],
          TraditionalForm]}], ",", " ", "\n", "  ", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"z", ",", " ", "2"}], "]"}], "[", "t", "]"}], " ", "->", 
         " ", 
         FormBox[
          RowBox[{"z", "[", "2", "]"}],
          TraditionalForm]}]}], "}"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"c", " ", "=", " ", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["p", "hip"], "]"}]}], " ", "/.", " ", "constsubs"}], 
       "]"}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"H", " ", "=", " ", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", 
           RowBox[{"a", ",", "2"}]], "]"}]}], " ", "/.", " ", "constsubs"}], 
       "]"}]}], ";", "\n", 
     RowBox[{"Phi", "=", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"fbz", "=", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
             RowBox[{"p", "[", "1", "]"}]}], ")"}], "/", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]}], "}"}], "\n", 
        RowBox[{"Subscript", "[", 
         RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "=", 
       RowBox[{
        RowBox[{"Transpose", "[", 
         RowBox[{"{", 
          RowBox[{"Table", "[", 
           RowBox[{
            RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
            RowBox[{"{", 
             RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
         "]"}], "/.", 
        RowBox[{"fbz", "\n", 
         RowBox[{"PhiI", "=", 
          RowBox[{"Simplify", "[", 
           RowBox[{
            RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
            RowBox[{"Join", "[", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{"Subscript", "[", 
                  RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}], "}"}], 
               "}"}], ",", 
              RowBox[{"Subscript", "[", 
               RowBox[{"yz", ",", "d", ",", "2"}], "]"}]}], "]"}]}], 
           "]"}]}]}]}]}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"PhidotI", "=", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
        RowBox[{"Join", "[", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"Subscript", "[", 
              RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}], "}"}], "}"}], 
          ",", 
          RowBox[{
           RowBox[{"D", "[", 
            RowBox[{
             RowBox[{"Subscript", "[", 
              RowBox[{"yz", ",", "d", ",", "2"}], "]"}], ",", 
             RowBox[{
              RowBox[{"Subscript", "[", 
               RowBox[{"z", ",", "1"}], "]"}], "[", "t", "]"}]}], "]"}], "*", 
           
           RowBox[{
            RowBox[{"Subscript", "[", 
             RowBox[{"z", ",", "2"}], "]"}], "[", "t", "]"}]}]}], "]"}]}], 
       "]"}]}], ";"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "write", " ", "the", " ", "control", " ", "files", " ", "to", " ", 
     "disk"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso\>\"", 
     "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{"hdot", "/.", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{"\[Sigma]y", "/.", "constsubs"}], "/.", "statesubs"}]}], "]"}],
     ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "hipdot"], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "hip"], "/.", "constsubs"}], "/.", "statesubs"}]}], 
     "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "COM"], "/.", "constsubs"}], "/.", "statesubs"}]}], 
     "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"d", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"d", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "1"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "2"}]], "/.", "constsubs"}], "/.", "statesubs"}]}],
      "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", "perl", "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"LineNumber", " ", "=", "2"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"FolderName", " ", "=", " ", 
     RowBox[{"\"\<build_torso_\>\"", "<>", 
      RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Run", "[", "\"\<perl math2mat_torso_1.pl\>\"", "]"}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Run", "[", 
     RowBox[{"\"\<mkdir \>\"", "<>", "FolderName"}], "]"}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"Run", "[", 
     RowBox[{
     "\"\<cp ./build_torso/*.m ./\>\"", "<>", " ", "FolderName", "<>", 
      "\"\</\>\""}], "]"}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "SetDirectory", "[", 
      "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs/build_torso\>\"",
       "]"}], ";"}], "*)"}], "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}}]
},
WindowSize->{1918, 1024},
WindowMargins->{{-9, Automatic}, {Automatic, -8}},
ShowSelection->True,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 93629, 2724, 5593, "Input"]
}
]
*)

(* End of internal cache information *)
